from .period_task import period_task
import lcm
from quad_bot import motors_info_pb, cheater_sensor_info_pb
import time

count = 0
num = 0


class lcm_publisher(period_task):
    def __init__(self, name, period, channel, channel2):
        period_task.__init__(self, name, period)
        self.lc = lcm.LCM()
        self.lc2 = lcm.LCM()
        self.channel = channel
        self.channel2 = channel2
        self.msg = motors_info_pb()
        self.msg2 = cheater_sensor_info_pb()

    def init(self):
        # print("[LCM publisher] init!")
        pass

    def run_once(self):
        # print("publisher run once")
        # global count, num
        # if count > 1000:
        #     count = 1
        #     num += 1
        # print("publisher run", num, count, "times")
        # count += 1
        self.lc.publish(self.channel, self.msg.encode())
        self.lc2.publish(self.channel2, self.msg2.encode())
        # print(time.time())
        pass

    def clean_up(self):
        print("[LCM publisher] clean up!")
        pass
